#include "NavMesh.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "DetourNavMeshQuery.h"
#include "DetourCommon.h"
#include "DetourNavMesh.h"
#include "platform.h"
#include "common.h"
#include <vector>

struct NavMeshSetHeader
{
	int version;
	int tileCount;
	dtNavMeshParams params;
};

struct NavMeshTileHeader
{
	dtTileRef tileRef;
	int dataSize;
};

NavMesh::NavMesh()
	:mNavMeshPointer(nullptr),
	mNavMeshQueryPointer(nullptr)
{
	mQueryFilterPointer = new dtQueryFilter();
	mQueryFilterPointer->setIncludeFlags(0xffff);
	mQueryFilterPointer->setExcludeFlags(0);
}

NavMesh::~NavMesh()
{
	if (mNavMeshPointer != nullptr)
	{
		dtFreeNavMesh(mNavMeshPointer);
		mNavMeshPointer = nullptr;
	}

	if (mNavMeshQueryPointer != nullptr)
	{
		dtFreeNavMeshQuery(mNavMeshQueryPointer);
		mNavMeshQueryPointer = nullptr;
	}

	if (mQueryFilterPointer != nullptr)
	{
		delete mQueryFilterPointer;
		mQueryFilterPointer = nullptr;
	}
}

bool NavMesh::Create(std::string path, int maxNodes, float extents[3], float wideExtents[3])
{
	FILE* fp = fopen(path.c_str(), "rb");
	if (!fp)
	{
		return false;
	}

	fseek(fp, 0, SEEK_END);
	size_t flen = ftell(fp);
	fseek(fp, 0, SEEK_SET);

	uint8* data = new uint8[flen];
	if (data == nullptr)
	{
		fclose(fp);
		SAFE_RELEASE_ARRAY(data);
		return false;
	}

	size_t readsize = fread(data, 1, flen, fp);
	if (readsize != flen)
	{
		fclose(fp);
		SAFE_RELEASE_ARRAY(data);
		return false;
	}

	if (readsize < sizeof(NavMeshSetHeader))
	{
		fclose(fp);
		SAFE_RELEASE_ARRAY(data);
		return false;
	}

	bool result = Create(data, flen, maxNodes, extents, wideExtents);
	fclose(fp);
	SAFE_RELEASE_ARRAY(data);

	return result;
}

bool NavMesh::Create(uint8* data, size_t flen, int maxNodes, float extents[3], float wideExtents[3])
{
	dtVcopy(mExtents, extents);
	dtVcopy(mWideExtents, wideExtents);

	bool safeStorage = true;
	int position = 0;
	int size = sizeof(NavMeshSetHeader);

	NavMeshSetHeader header;
	memcpy(&header, data, size);

	position += size;

	if (header.version != RCN_NAVMESH_VERSION)
	{
		return false;
	}

	dtNavMesh* navMeshPointer = dtAllocNavMesh();
	if (!navMeshPointer)
	{
		return false;
	}

	dtStatus status = navMeshPointer->init(&header.params);
	if (dtStatusFailed(status))
	{
		return false;
	}

	bool success = true;
	for (int i = 0; i < header.tileCount; ++i)
	{
		NavMeshTileHeader tileHeader;
		size = sizeof(NavMeshTileHeader);
		memcpy(&tileHeader, &data[position], size);
		position += size;

		size = tileHeader.dataSize;
		if (!tileHeader.tileRef || !tileHeader.dataSize)
		{
			success = false;
			status = DT_FAILURE + DT_INVALID_PARAM;
			break;
		}

		unsigned char* tileData = (unsigned char*)dtAlloc(size, DT_ALLOC_PERM);
		if (!tileData)
		{
			success = false;
			status = DT_FAILURE + DT_OUT_OF_MEMORY;
			break;
		}

		memcpy(tileData, &data[position], size);
		position += size;

		status = navMeshPointer->addTile(tileData
			, size
			, (safeStorage ? DT_TILE_FREE_DATA : 0)
			, tileHeader.tileRef
			, 0);

		if (dtStatusFailed(status))
		{
			success = false;
			break;
		}
	}

	if (!success)
	{
		dtFreeNavMesh(navMeshPointer);
		return false;
	}

	dtNavMeshQuery* pMavmeshQuery = new dtNavMeshQuery();

	pMavmeshQuery->init(navMeshPointer, maxNodes);

	mNavMeshQueryPointer = pMavmeshQuery;
	mNavMeshPointer = navMeshPointer;

	uint32 tileCount = 0;
	uint32 nodeCount = 0;
	uint32 polyCount = 0;
	uint32 vertCount = 0;
	uint32 triCount = 0;
	uint32 triVertCount = 0;
	uint32 dataSize = 0;

	const dtNavMesh* navmesh = navMeshPointer;
	for (int32 i = 0; i < navmesh->getMaxTiles(); ++i)
	{
		const dtMeshTile* tile = navmesh->getTile(i);
		if (!tile || !tile->header)
		{
			continue;
		}

		tileCount++;
		nodeCount += tile->header->bvNodeCount;
		polyCount += tile->header->polyCount;
		vertCount += tile->header->vertCount;
		triCount += tile->header->detailTriCount;
		triVertCount += tile->header->detailVertCount;
		dataSize += tile->dataSize;
	}

	return true;
}

int NavMesh::FindStraightPath(const float* startPoint, const float* endPoint, Vector3 straightPathPoints[])
{
	NavMeshPoint startNavMeshPoint;
	NavMeshPoint endNavMeshPoint;
	GetNavMeshPoint(startPoint[0], startPoint[1], startPoint[2], &startNavMeshPoint);
	GetNavMeshPoint(endPoint[0], endPoint[1], endPoint[2], &endNavMeshPoint);

	if (startNavMeshPoint.polyRef == 0 || endNavMeshPoint.polyRef == 0)
	{
		return -1;
	}

	int nPathCount;
	int nStraightPathCount = 0;
	mNavMeshQueryPointer->findPath(startNavMeshPoint.polyRef, endNavMeshPoint.polyRef, startNavMeshPoint.point, endNavMeshPoint.point, mQueryFilterPointer, mPathPolyBuffer, &nPathCount, Max_Path_Size);

	if (nPathCount > 0)
	{
		float closestNavMeshPoint[3];
		dtVcopy(closestNavMeshPoint, endNavMeshPoint.point);

		if (mPathPolyBuffer[nPathCount - 1] != endNavMeshPoint.polyRef)
		{
			mNavMeshQueryPointer->closestPointOnPoly(mPathPolyBuffer[nPathCount - 1], endNavMeshPoint.point, closestNavMeshPoint, 0);
		}
		mNavMeshQueryPointer->findStraightPath(startNavMeshPoint.point, endNavMeshPoint.point, mPathPolyBuffer, nPathCount, mStraightPathBuffer, mStraightPathFlagBuffer, mStraightPathPolyBuffer, &nStraightPathCount, Max_Path_Size);

		for (int i = 0; i < nStraightPathCount; ++i)
		{
			Vector3 currentPosition;
			currentPosition.x = mStraightPathBuffer[i * 3];
			currentPosition.y = mStraightPathBuffer[i * 3 + 1];
			currentPosition.z = mStraightPathBuffer[i * 3 + 2];
			mStraightPathPointBuffer[i] = currentPosition;
			straightPathPoints[i] = currentPosition;
		}

		if (nStraightPathCount == 1)
		{
			mStraightPathPointBuffer[1] = mStraightPathPointBuffer[0];
			straightPathPoints[1] = straightPathPoints[0];
			nStraightPathCount = 2;
		}
	}

	return nStraightPathCount;
}

bool NavMesh::GetNavMeshPoint(float x, float y, float z, float extents[3], NavMeshPoint* navMeshPoint)
{
	if (mNavMeshQueryPointer != nullptr)
	{
		float center[3];
		center[0] = x;
		center[1] = y;
		center[2] = z;
		dtStatus status = mNavMeshQueryPointer->findNearestPoly(center, extents, mQueryFilterPointer, &navMeshPoint->polyRef, navMeshPoint->point);
		if (dtStatusSucceed(status) && navMeshPoint->polyRef != 0)
		{
			return true;
		}
	}

	return false;
}

bool NavMesh::GetNavMeshPoint(float x, float y, float z, NavMeshPoint* navMeshPoint)
{
	if (GetNavMeshPoint(x, y, z, mExtents, navMeshPoint))
	{
		return true;
	}
	else
	{
		if (GetNavMeshPoint(x, y, z, mWideExtents, navMeshPoint))
		{
			return true;
		}
	}

	return false;
}

bool NavMesh::GetPolyHeight(float x, float y, float z, float* height)
{
	NavMeshPoint navMeshPoint;
	if (GetNavMeshPoint(x, y, z, &navMeshPoint))
	{
		dtStatus status = mNavMeshQueryPointer->getPolyHeight(navMeshPoint.polyRef, navMeshPoint.point, height);
		if (dtStatusSucceed(status))
		{
			return true;
		}
	}

	return false;
}

bool NavMesh::FindNearestPoly(float x, float y, float z, bool useWideExtents, float* resultPoint)
{
	float center[3] = { x,y,z };
	dtPolyRef polyRef = 0;
	dtStatus status = mNavMeshQueryPointer->findNearestPoly(center, mExtents, mQueryFilterPointer, &polyRef, resultPoint);
	if (dtStatusSucceed(status) && polyRef != 0)
	{
		return true;
	}
	else if (useWideExtents)
	{
		status = mNavMeshQueryPointer->findNearestPoly(center, mWideExtents, mQueryFilterPointer, &polyRef, resultPoint);
		if (dtStatusSucceed(status) && polyRef != 0)
		{
			return true;
		}
	}

	return false;
}

bool NavMesh::Raycast(float x1, float y1, float z1, float x2, float y2, float z2)
{
	if (mNavMeshPointer != nullptr && mNavMeshQueryPointer != nullptr && mQueryFilterPointer != nullptr)
	{
		NavMeshPoint startNavMeshPoint;
		if (GetNavMeshPoint(x1, y1, z1, &startNavMeshPoint))
		{
			float endPoint[3];
			endPoint[0] = x2;
			endPoint[1] = y2;
			endPoint[2] = z2;

			float hitParameter = 0.0f;
			float hitNormal[3];
			dtStatus status = mNavMeshQueryPointer->raycast(startNavMeshPoint.polyRef, startNavMeshPoint.point, endPoint, mQueryFilterPointer, &hitParameter, hitNormal, mRaycastPathPolyBuffer, &mRaycastPathCount, Max_Path_Size);
			if (dtStatusSucceed(status))
			{
				if (hitParameter > 1)
				{
					return true;
				}
			}
		}
	}

	return false;
}

bool NavMesh::MoveAlongSurface(float x1, float y1, float z1, float x2, float y2, float z2, float resultPoint[3])
{
	if (mNavMeshPointer != nullptr && mNavMeshQueryPointer != nullptr && mQueryFilterPointer != nullptr)
	{
		NavMeshPoint startNavMeshPoint;
		if (GetNavMeshPoint(x1, y1, z1, &startNavMeshPoint))
		{
			float endPoint[3];
			endPoint[0] = x2;
			endPoint[1] = y2;
			endPoint[2] = z2;

			dtStatus status = mNavMeshQueryPointer->moveAlongSurface(startNavMeshPoint.polyRef, startNavMeshPoint.point, endPoint, mQueryFilterPointer, resultPoint, mVisitedPolyBuffer, &mVisitedCount, Max_Path_Size);
			if (dtStatusSucceed(status))
			{
				return true;
			}
		}
	}

	return false;
}
